//
//  trackball.cpp
//  TreeViewerIOS
//
//  Created by Billy Clack on 10/13/12.
//  Copyright (c) 2012 Billy Clack. All rights reserved.
//

#include "trackball.h"

#include <cmath>

//Width height of the screen.
TrackBall::TrackBall(unsigned int w, unsigned int h) {
	InitTrackBall(w, h);
}

TrackBall::TrackBall() {
	InitTrackBall(512, 512);
}

void TrackBall::InitTrackBall(unsigned int w, unsigned int h){
   //A trackball centered in the screen, assuming width/height of screen is 400x400.
    c = glm::vec3(0.0, 0.0, 0.0);
    r = w;
    rsq = r*r;
    screenW = w;
    screenH = h;
    lastP = glm::vec3(0,0,0);
	mRot = glm::quat();
	//mRot initialization -- identity quaternion
}

void TrackBall::GetCurrentRotation(float * outmat) {
	//fills a matrix with a gl matrix representation of the current rotation.
	glm::mat4 rotmat = glm::mat4_cast(mRot);
	for (int i = 0; i < 16; ++i){ outmat[i] = *(&rotmat[0][0]+i); }
}

glm::quat TrackBall::GetCurrentRotation(){
    return mRot;
}
    
void TrackBall::SetPoint( unsigned int sx, unsigned int sy){
    lastP = GetPointFromScreenPos( (float)sx, (float)sy );
}

void TrackBall::UpdatePoint( unsigned int sx, unsigned int sy ){
    
    glm::vec3 currPoint = GetPointFromScreenPos((float)sx, (float)sy);

    //Accumulate using current rotation and mRot.
	glm::quat currRot = GetRotation(currPoint) * mRot;
    
    //Normalize mRot
    mRot = glm::normalize(currRot);

    //Update points
    lastP = currPoint;
    
}

//Normalized
glm::quat TrackBall::GetRotation( const glm::vec3 & currPoint ) {
    
    glm::vec3 diff = currPoint - lastP;
        if (glm::dot(diff, diff) <= 0.00001){
            return glm::quat();
        }
    
        glm::vec3 v1 = glm::normalize(lastP);
        glm::vec3 v2 = glm::normalize(currPoint);
    
        //Axis
		glm::vec3 n = glm::normalize(glm::cross(v1, v2));
        
        //Angle
        float phi = std::acos(glm::dot(v1, v2));
		float sinphihalf = std::sin(phi*0.5);
		glm::vec3 quatxyz = n*sinphihalf;
        glm::quat q = glm::quat(std::cos(phi*0.5f), quatxyz.x, quatxyz.y, quatxyz.z);
        
    return glm::normalize(q);
}

//Gets the projected point on the sphere/hyperbolic sheet from the screen
//position
glm::vec3 TrackBall::GetPointFromScreenPos( float sx, float sy){
    float xMod = (sx - (screenW*0.5) - c.x),
        yMod = -1*(sy - (screenH*0.5) - c.y); //Multiply by -1 since the origin (00) is bottom left.
    
    //check if we need a trackball or a hyperbolic sheet.
    float sdot = xMod*xMod + yMod*yMod;
    float zMod;
    if (rsq*0.5 >= sdot){
        //Use sphere
        zMod = std::sqrt(rsq - sdot);
    }
    else {
        //Use hyperbolic sheet.
        zMod = (rsq*0.5f)/(std::sqrt(sdot));
    }
    
    return glm::vec3(xMod, yMod, zMod);

}
   
